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I.  INTRODUCTION 

The  problem  discussed  in  this  paper  is  that  of  esti- 
mating the  position  and  velocity  in  two  dimensions  of  a 
target  by  means  of  processing  passively  obtained  bearing 
measurements . 

A  single  moving  observer  (tracker)  monitors  noisy  sonar 
bearings  from  a  radiating  acoustic  source  (target).  The 
geometric  configuration  is  depicted  in  Figure  1.1. 

The  problem  contains  nonlinearities  so  the  conventional 
linear  analysis  is  not  possible.  Also  as  it  will  be  shown  in 
chapter  IV  the  dynamic  process  remains  unobservable  prior  to 
tracker  maneuver.  That  requirement  of  observer  maneuvering 
distinguishes  this  problem  from  the  more  usual  target  motion 
analysis  (TMA)  problem. 

In  chapter  II  the  basic  concept  of  the  Kalman  filter  is 
described.  Chapter  III  describes  the  non- linear  case 
(Extended  Kalman  filter)  in  which  category  the  bearings  only 
tracking  problem  belongs. 

In  chapters  IV  and  V  the  problem  of  bearings  only 
tracking  with  nonmaneuvering  and  maneuvering  targets  is 
discussed.  Some  possible  solutions  from  the  literature  are 
referenced,  and  the  case  of  solving  the  problem  through  a 
specific  approach,  i.e  by  using  a  variable  value  of  the 
system's  noise  covariance  matrix  "Q"  is  tested. 

Chapter  VI  contains  the  results  of  the  computer  simula- 
tions on  the  subject  and  chapter  VII  contains  conclusions 
and  possible  topics  for  further  investigation. 


Y 

4 


TRACKER 


TARGET 


-#    X 


Figure    1.1         Geometrical    Configuration    for    che    B.O.T    Problem 
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II.  KALMAN  FILTERING  BASICS 


HISTORY 


In  1960,  R.E.  Kalman  provided  a  new  way  of  formulating 
the  least  squares  filtering  problem  using  state-space 
methods  [Ref.  1] .  Until  that  time  the  Wiener  solution  of 
the  optimal  filter  problem  was  applied  ,  which  was  using  the 
concept  of  the  "weighting  function".  In  effect  the  weighting 
function  tells  how  the  past  values  of  the  input  should  be 
weighted  in  order  to  determine  the  present  value  of  the 
output,  that  is  the  optimal  estimate.  But  the  Wiener  solu- 
tion did  not  lend  itself  very  well  to  the  corresponding 
discrete-data  problem  nor  was  it  easily  extended  to  more 
complicated  problems  [Ref.  2]. 

The  two  main  features  of  the  Kalman  formulation  and 
solution  of  the  problem  are: 

Vector  modeling  of  the  random  processes  under 
consideration. 

Recursive  processing  of  the  noisy  measurements 
(input  data).  The  key  element  in  any  recursive  procedure  is 
the  use  of  the  results  of  the  previous  step  to  aid  in 
obtaining  the  desired  result  for  the  current  step.  This  is 
the  main  feature  of  the  Kalman  filtering  and  the  one  that 
clearly  distinguishes  it  from  the  weighting  function 
approach,  which  requires  arithmetic  operations  on  all  the 
past  data. 

The  Kalman  filtering  technique  has  become  very  popular 
in  target  tracking  applications  for  the  previous  reasons 
plus  the  following: 
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At  a  given  time  t,  the  filter  generates  an  unbi- 
ased estimate  of  the  state  vector, which  means  that  the 
expected  value  of  the  estimate  is  the  value  of  the  state 
vector  at  time  t. 

The  estimate  is  a  minimum  variance  estimate 
meaning  that  it  has  the  property  that  its  error  covariance 
is  less  than  or  equal  to  that  of  any  other  linear  unbiased 
estimate . 

The  filter  is  linear  and  simplifies  the  calcula- 
tions [Ref.  3]. 

B.   THE  DISCRETE  KALMAN  FILTER 

Assume  that   the  random  process   to  be  estimated   can  be 
modeled  in  the  form: 

x(k+l)=  0(k)x(k)  +Tw(k)  +Au(k)  (2.1) 

and  the  observation  or  measurement  of  the  process  is  assumed 
to  occur  at  discrete  points  in  time  in  accordance  with  the 
relationship: 

z(k)  =  H(k)x(k)  +  n(k)  (2.2) 

where : 

x(k)  =  (nxl)  ;  is  the  process  state  at  time  t(k) 

O(k)   =  (nxn)    ;   is  the  matrix   relating  x(k)   to 
x(k+l)  in  the  absence  of  forcing  function. 

w(k)  ;  is  the  random  forcing  input  at  time  t(k) 
considered  to  be  an  uncorrelated  sequence  with  zero  mean  and 
known  variance. 
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{"(k)   =  (nxp)   ;   is  the  matrix  relating  the  random 
forcing  inputs  to  the  state  at  time  t(k).1 


u(k)   ;   is  the  deterministic  forcing  input  at  time 


t(k) 


/^(k)  =  (nxp)  ;  is  the  matrix  relating  the  determin- 
istic inputs  to  the  state  at  time  t(t). 


z(k)   =  (mxl)   ;   is  the  measurement  vector  at  time 


t(k) 


H(k)  =  (mxn)  ;  is  the  matrix  which  gives  the  noise- 
less connection  between  the  state  vector  and  the  measurement 
equation  at  time   t(k). 


n(k)  =  (mxl)  ;  is  the  measurement  noise  error  which 
is  assumed  to  be  a  white  sequence  with  known  covariance 
structure  and  uncorrelated  with  the  w(k)  sequence. 


The  corresponding  covariance  matrices  are  given  by 


.  2 


Qoo 


E[w  w,' ]  = 


E[n  n']  = 


fc=  i 


K*t 


Rco   *=t 


I     o  **<- 


(2.3) 


(2.4) 


E[w  n,']  =  0   for  all  k  and  i 


(2.5) 


*p<n 

2  (')  denotes  matrix  transposition 
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It  is  assumed  that  we  have  available  an  initial  estimate 
of  the  process  at  time  t(k),  which  is  based  on  the  knowledge 
about  the  process  prior  to  time  t(k).  This  prior  estimate 
will  be  denoted  as  x(k/k-l)  where  the  "hat"  denotes  esti- 
mate, and  the  (k/k-1)  subscript  means  that  this  is  our  esti- 
mate prior  to  processing  the  measurement  at  time  t(k). 

With  the  assumption  of  the  prior  estimate  x(k/k-l),  we 
now  seek  to  use  the  measurement  z(k)  to  improve  the  esti- 
mate. To  do  that  we  choose  a  linear  blending  of  the  received 
noisy  measurement  and  the  prior  estimate  in  accordance  with 
the  equation: 

x(k/k)  =  x(k/k-l)+G(k)[z(k)-H(k)x(k/k-l)]  (2.6) 

where  x(k/k)  is  the  update  estimate,  x(k/k-l)  is  given  by: 

x(k/k-l)  =(£(k)x(k-l/k-l)  (2.7) 

and  the  G(k)  is  the  blending  factor.  G(k)  is  going  to  be 
determined  later.  At  this  time  the  problem  is  to  find  a 
particular  value  of  G(k)  that  yields  an  update  estimate  that 
is  optimal  in  s  ne  sense.  The  minimum  Mean  -  Square  error 
is  the  require  performance  criterion  for  that  "optimiza- 
tion". To  do  that  we  need  to  define  the  term  "error  covari- 
ance  matrix"  P(k),  associated  with  the  update  (a  posteriori) 
estimate,  which  is  a  matrix  representing  the  covariance  of 
the  difference  between  the  true  state  vector  x(k)  and  the 
estimated  x(k) . 

P(k)  =  E  [(x(k)-x(k/k-l))(x(k)-x(k/k-l))']  (2.8) 

The  optimization  can  be  done  by  various  mathematical  ways  as 
treated  in  [Ref.  4]  [Ref.  2]  and  [Ref.  5].  The  mathematical 
derivation  which  is  omitted  here  shows  that  if 
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G(k)=P(k/k-l)H' (k)[H(k)P(k/k-l)H* (k)  +R(k)]         (2.9) 

then  this  is  the  G(k)  that  minimizes  the  mean  square  estima- 
tion error,  and  it  is  called  the  "Kalman  gain"  [Ref.  2]. 

Next  the  covariance  matrix  associated  with  the  optimal 
estimate  may  be  computed  and  is  given  by:3 

P(k/k)  =  [I  -  G(k)H(k)]P(k/k-l)  (2.10) 

Now  the  updated  estimate  x(k/k)  can  be  easily  projected 
ahead  via  the  transition  matrix  by  the  equation: 

x(k+l/k)  =<^)(k)x(k/k)  (2.11) 

ignoring  the  contribution  of  w(k)    because  it  has  zero  mean 
and  also  it  is  uncorrelated  with  the  previous  W's. 
Also, the  equation 

P(k+l/k)  =0(k)P(k/k)0'(k)+Q(k)  (2.12) 

closes  the  loop  and  now,  having  the  needed  quantities  for 
the  next  moment  with  the  next  measurement  we  can  start  again 
as  in  the  previous  steps. 

Equations  (2.6),  (2,9),  (2,10),  (2,11),  and  (2,12)  thus 
comprise  the  Kalman  filter  recursive  equation  set. 

In  Figure  2.1  the  Kalman  filter  loop  is  indicated. 

1 .   A  Simple  Example 

Assume  that  a  stationary  tracker  has  the  ability  to 
obtain  range  measurements  in  both  X  and  Y  directions  of  a 
target  moving  as  in  Figure  2.2. 


3 (I)  is  the  identity  matrix. 
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Ealet  JPL lac _asllma L£_  x.ls/k_-l 1  jqo.  lt&_  arxar_  ravflCLlIoci 


i 

< 

1 

* 

Compute  Kalman  gatn;                       -i 
G(k)-P(k/k-l)H,(k)LH(k)P(K/k-l)H,(k)*R(k)] 

( 

E  at_ec  jneasticenieni_  zlk  1' 

< 

Project  ahead: 
*(k*l/k)«0(k)x(k/k)   and 

P(k*l/k)-O(kJP(k/k)0'(k)»0 
(k) 

update  estimate  *tth 

measurement   z(k): 

■*(k/k)-x(k/k-l  j*G(k)tz(k)- 

H(k)x(k/k-l)3 

i 

i 

Compute  error   covartance  for   updated 
estimate:   P(k/kW  I-G(k)H(k)  ]P(k/k- 1 ) 

„ 

Figure  2.1    The  Kalman  Filter  Loop. 
Let   the  target   be  moving   with  a   tangential  velocity   of 

o 

1,660  m/min  so  that  it  covers  the  arc  of  90  in  10  minutes. 
The  tracker  makes  its  measurements  every  1  min.  It  is 
desired  to  estimate  the  state  vector  of  the  target  defined 
as  X,VX,Y,  and  Vy  ,  i.e.,  range  and  velocity  in  X  and  Y 
directions.  Given  are:  an  initial  estimate  x(k/k-l)  and  its 
error  covariance  matrix  P(k/k-l).  Let  them  be: 
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Figure    2.2        Tracker-Target    Configuration   in   Example 
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x(k/k-l)    = 


10000 

o 
o 

1600 


(2.13) 


and 


P(k/k-l)    = 


1000    0  0  o 

o  1000  0  o 

o  o  1 0  0  0  o 

o  o  o  IOOO 


(2.14) 


Then  we   can  calculate   the  Kalman  filter   gain  G(k)    as  in 
equation  (2.9)  where: 


H(k)    =    constant    = 


IOOO 

o     o       i      o 


(2.15) 


and  R(k)  has  the  value 


R(k)  =  constant  = 


o  i 


(2.16) 


Next,  given  the  measurement,   the  updated  estimate  is  calcu 
lated  using  equation  (2.6)  where: 


z(k)  = 


2« 


(2.17) 


We  can  see  here  that  the  updated  estimate  x(k/k)   depends  on 
the  previous  x(k-l/k-l)  propagated  for  the  instant  (k)  i.e., 
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x(k/k-l),  and  another  portion  equal  to  G(k) [z (k) -H(k)x(k)] . 
That  second  portion  depends  on  the  G(k)  and  on  how  much  the 
estimated  and  the  received  measurements  differ. 

The  updated  error  covariance  matrix  P(k/k)  is  then 
computed  using  equation  (2.10).  The  updated  error  covariance 
P(k/k)  is  going  to  be  less  than  the  previous  P(k/k-l)  since 
the  filter  processed  an  observation  and  thus  the  uncertainty 
about  the  estimate  is  less.  The  term  [I-G(k)H(k)]  is  always 
less  than  unity  if  G(k)  is  not  zero.  That  means  that  if  we 
used  the  last  observation  (i.e.,  G(k)  not  zero)  then  the 
term  in  the  brackets  is  less  than  unity  and  P(k)  becomes 
less  than  P(k/k-l) . 

Now  having  x(k/k)  and  P(k/k)  we  must  propagate  them 
for  the  next  instant  when  the  next  measurement  will  be  taken 
in  order  to  be  able  to  compare  it  with  the  real  one  through 
the  new  measurement.  So  we  project  ahead  our  estimate  by 
the  equation  (2.11)  where: 


<J(k)  = 


/  /  o  o 

O  I  o  o 

o  0  I  i 

o  o  o  I 


(2.18) 


and  the  error  covariance  matrix  by  equation  (2.12)  with  Q(k) 
such  that: 


Q(k)  =   r(k)E[w(k)w'(k)]f'(k) 


(2.19) 


where 
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T(k)  = 


T  0 


(2.20) 


and  w(k)  is  the  random  forcing  input  at  time  t(k)  which  is 
to  be  formulated  as  a  white  noise  with  known  variance. 
Finally  Q(k)  is  given  by: 


Q(k)  = 


r       ** 

o    o 


r 

4 


_     T 


a. 


,  wCk) 


(2.21) 


and  it  counts  for  the  uncertainty  introduced  by  the  fact 
that  we  do  not  know  if  the  target  during  the  next  coming 
time  interval  will  maneuver  or  not.  A  big  value  of  w(k) 
means  that  the  target  is  very  likely  during  the  propagation 
interval  to  maneuver.  In  this  case  the  Kalman  gain  will  also 
be  large  and  the  filter  will  weight  the  observation  moire 
than  the  propagated  state.  On  the  opposite  case  if  w(k)  is 
zero  the  filter  assumes  that  the  target  did  not  maneuver 
during  that  interval  so  it  weights  more  the  last  estimate 
than  the  measurement.  In  the  above  case  it  is  also  assumed 
that  the  target  acceleration  in  X  direction  is  uncorrelated 
to  the  acceleration  in  Y  direction  for  simplicity. 

Having  the  propagated  values  of  x(k+l/k)  and 
P(k+l/k)  we  can  start  over  again  from  the  initial  step. 

The  above  algorithm  was  simulated  in  the  computer. 
The  interesting  result  obtained  is  that  for  the  case  that 
the  target  maneuvers  the  choice  of  w(k)  is  very  important. 
If  it  is  small  or  zero  the  filter  does  not  include  any  extra 
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uncertainty  due  to  possible  target  maneuver.  So  at  any 
moment  it  gives  more  weight  to  the  last  propagated  estima- 
tion and  less  to  the  received  measurement.  Thus  the  tracking 
accuracy  is  not  good  compared  with  that  in  which  it  includes 
uncertainty  as  can  be  seen  in  the  results  shown  in  Figures 
2.3,  2.4,  2.5,  2.6,  and  2.7. 
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III.  NONLINEAR  ESTIMATION 


A.   INTRODUCTION 


The  majority  of  physical  phenomena  are  nonlinear  in 
nature.  So  as  a  result  , usually  ,the  state  and/or  measure- 
ment equations  are  nonlinear.  Since  the  basic  Kalman  filter 
theory  deals  with  linear  cases,  it  is  necessary  to  find  a 
"method"  to  use  it  in  nonlinear  estimation  problems. 

There  are  two  ways  of  solving  that  problem:   [Ref.  4]. 

1.  By  deriving  an  optimal  filter  for  the  nonlinear 
problem  or 

2.  By  linearizing  (approximating)   the  problem  and 
applying  the  linear  filter  theory. 

The  first  method  is  hard  to  follow  and  will  involve 
complicated  mathematical  computations.  On  the  other  hand  the 
second  method  is  easier  and  the  more  usual.  For  the 
reasons  above  the  second  method  will  be  followed  in  this  and 
the  following  chapters. 

B.   ANALYSIS 

In  the  following  analysis  it  is  assumed  that  both  the 
state  and  the  measurement  equations  are  nonlinear  although 
this  is  not  always  the  case. 

Assume  that  the  random  process  to  be  estimated  can  be 
modeled  by: 

x(k+l)  =  a[x(k),u(k),k]  +  w(k)  (3.1) 

with  the  measurement  equation: 

z(k)  =    c[x(k)]  +  n(k)  (3.2) 
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It  is  necessary  to  have  available  a  nominal  trajectory 
x  (k) ,  k=0,l,2,....  about  which  the  linearization  will  be 
performed.  The  vector  function  a[x(k) ,u (k) ,k]  is  expanded  in 
Taylor  series  about  the  nominal  trajectory  x  (k) .  Then  the 
linearized  state  equations  can  be  written: 


x(k+l)  =  a[xC0,)(k),u(k),k]+  <9a 


3* 


[x(k)-x   (k)]+w(k)    (3.3) 


[x   (k),u(k),k] 


If  A(k)  is  defined  to  be  the  first  partial  derivative  of  the 
nonlinear  function  a[x  (k),u(k),k],  with  respect  to  the 
state  vector  x(k) ,  i.e., 


A(k)  = 


da 


3, 


[xf°CK),u  (■<>,*] 


Then,  the  ijth  entry  of  matrix  A  is  given  by 


(3.4) 


(A)     *      9«: 


1-3 


3, 


IV'Vk^ocXk] 


CO 


(3.5) 


Also  , the  vector  function  a[x  (k),u(k),k]  is  a  known 
function  of  k.  Thus  the  linearized  state  equations  can  be 
written  as : 


x(k+l)=A(k)x(k)  +  a[xfo)(k),u(k),k] 
-  A(k)x<°(k)+w(k) 


(3.6) 


The  accuracy  of  this  approximation   depends  on  how  close 
the  nominal  trajectory  to  the  actual  one  was  selected. 

Let  us  now  consider  the  measurement  equation.  We  have: 


z(k)  =  c[x(k)]  +n(k) 


(3.7) 
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Again  we   can  expand  the   nonlinear  vector  function   c  about 
the  nominal  trajectory  x   (k)  with  the  result: 


» 


k)1+  3c 

3, 


Defining 


H(k)  = 


3c 


3, 


we  can  write 


[x(k)-x(0>(k)]+n(k) 


x(o)(k) 


(o) 


CW) 


(3.8) 


(3.9) 


(o) 


z(k)=H(k)x(k)  +  c[x(o)(k)]  -H(k)xCo)(k)+n(k) 


(3.10) 


Again  as  in  the  linearized  state  equation,  the  two  terms  in 
the  middle  of  the  equation  (3.10)  are  known  quantities  and 
they  can  be  handled  as  if  they  were  a  time  varying  but  known 
measurement  bias.   For  simplification  if  we  will  define" 


(o) 


u'(k)  =  a[xko;(k),u(k),k]  -  A(k)  xlo;(k) 


<o) 


(3.11) 


and 


z'(k)  =  z(k)-c  [x(°>(k)]  +H(k)x(0)(k)  =  z(k)-^(k)     (3.12) 


where : 


u ( ' )  in  this  case  means  "prime" 
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f(k)  =  cr   (o>(k)  -  H(k)x(<r>(k)]  (3.13) 

we  can  rewrite  equations  (3.6)  and  (3.7)  as: 

x(k+l)  =  A(k)x(k)  +  u(k)  +  w(k)  (3.14) 

and 

z(k)  =  H(k)x(k)  +  f (k)  +  n(k)  (3.15) 

Then  starting   with  these  linearized  equations,    the  appro- 
priate Kalman  filter  equations  are: 

the  gain  equation: 

-i 
G(k)  =  P(k/k-l)H' (k)[H(k)P(k/k-l)H' (k)+R(k)]       (3.16) 

the  covariance  of  estimation  error  equations: 
P(k/K-1)  =  A(k-l)P(k-l/k-l)A' (k-l)+Q(k-l)  (3.17) 

P(k/k)  =  [I-G(k)H(k)]  P(k/k-l)  (3.18) 

the  filter  update  equation: 
x(k/k)  =    x(k/k-l)+G(k)[z(k)-c(x(k/k-l))]  (3.19) 

and  the  prediction  equation: 

x(k+l/k)  =  a[x(k/k) ,u(k) ,k]  (3.20) 

Notice  that  in  equations  (3.19)  and  (3.20)  the  nonlinear 
state  and  measurement  relationships  are  used.  An  alternative 
is  to  use  the  linearize  relationships  in  which  case  we 
have : 
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x(k/k)  =  x(k/k-l)+G(k)[z(k)-H(k)x(k/k-l)]         (3.21) 
and 

x(k+l/k)  =  A(k)x(k/k)  +  u(k)  (3.22) 

One  question  to  be  answered  now  is  how  to  determine  the 
"nominal"  trajectory  used  before.  One  way  is  to  use  an 
approximate  trajectory  that  is  known  in  advance.  This 
trajectory  may  be  available  from  known  data,  or  may  have 
been  computed  by  solving  the  state  equations: 

x(°\k+l)  =  a[x(o)(k),u(k),k]  (3.23) 

with  the  initial  condition  x(o)(0)  =  E[x(0)].  Unfortunately, 
if  the  uncertainty  in  x(0)  is  large  the  solution  of  equation 
(3.23)  may  be  "too  far"  from  x(k),  the  linerization  error 
too  big  and  the  whole  method  inaccurate. 


C.   THE  EXTENDED  KALMAN  FILTER 

Another  possibility  is  to  use  the  estimates  produced  by 
the  filter  as  the  nominal  trajectory  about  which  the  linear- 
ization is  performed.  The  estimator  equations  are  again 
given  by  equations  (3.21)  and  (3.22).  The  matrices  A(k)  and 
H(k)  must  be  used  to  generate  G(k)  so  that  it  is  available 
to  process  z(k)  when  it  is  available.  Thus  the  best  informa- 
tion we  have  when  H(k)  must  be  evaluated  is  x(k/k-l);  when 
A(k)  is  to  be  evaluated,  however,  x(k/k)  is  available. 
Hence : 
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A(k)  t-A 


(3.24) 


[x(KU)ya(w\k] 


and 


H(k)  = 


3c 


(3.25) 


[x(kU-0] 


The   H(k)   and  A(k)    matrices   must  be  computed  on-line 
and  not  in  advance  since  they  depend  on  the  last  estimate. 
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IV.  BEARINGS  ONLY  TARGET  MOTION  ANALYSIS  -  NONMANEUVERING 

TARGET 

A.  PROBLEM  DEFINITION 

The  problem  considered  here  is  that  of  estimating  the 
position  and  velocity  of  a  target,  in  two  dimensions,  by 
processing  passively  obtained  bearing  measurements. 

The  main  application  area  is  the  Antisubmarine  Warfare 
area  where  either  a  surface  ship  tries  to  locate  a  submarine 
through  its  cavitation  noise  or  sonar  transmissions,  or  vice 
versa . 

In  the  following  discussion  we  will  consider  a  moving 
observer  (own  ship)  that  monitors  noisy  sonar  bearings  to  an 
acoustic  source  (target),  and  processes  these  measurements 
to  obtain  estimates  of  target  position  and  velocity. The 
geometric  configuration  is  shown  in  Figure  1.1. 

B.  FORMULATIONS  OF  THE  PROBLEM 

As  it  was  mentioned  earlier  the  problem  contains  nonli- 
nearities,  and  the  linear  Kalman  filter  is  not  applicable. 
Depending  on  the  selection  of  the  working  coordinate  system 
the  nonlinear  term  may  be  either  the  state  equation  or  the 
measurement  equation.  Even  models  with  mixed  elements  from 
different  coordinate  systems  have  been  used.  Following  are 
the  most  commonly  used  formulations  of  the  problem: 

1 .   Modified  Polar  Coordinates 

In  the   modified  polar   (MP)   coordinates   the  state 
vector  is  comprised  of  the  following   components: 
.  Bearing 
.  Bearing  rate 
.  Range  rate  divided  by  range 
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.  The  reciprocal  of  range. 
In  this  case  the  measurement  equation  is  linear  and  the 
state  equation  nonlinear.  The  nonlinearities  exhibited  by 
the  state  equations  are  considerably  more  complicated  than 
those  exhibited  by  a  formulation  where  the  measurement  equa- 
tion is  the  nonlinear.  Consequently  the  computational  load 
for  this  formulation  is  increased.  Details  about  the  modi- 
fied polar  coordinates  formulation  can  be  found  in  [Ref.  6]. 

2 .   Mixed  Coordinates 

In  this  case  as  in  the  previous  one  the  surement 
equation  is  the  linear  one  and  the  state  eq_  ion  non 
linear.   The  state  vector  consists  of: 

.  Bearing 

.  Range 

.  Velocity  component  in  x-direction 

.  Velocity  component  in  y-direction 
Again  in   this  formulation  there   is  the  same   complexity  in 
linearizing   the  state   equation   as   well  as   computational 
load.    Analysis  of  the   mixed- coordinate  formulation  can  be 
found  in  [Ref.  7]. 

Pseudo- Linear  Formulation 

This  formulation  involves  replacing  of  the  measured 
bearings  with  pseudo- linear  measurement  residuals,  to 
decouple  the  covariance  computations  from  the  estimated 
solution.  The  attractive  feature  of  this  method  is  that  it 
permits  a  solution  to  the  problem  via  linear  estimation 
techniques.  This  formulation  is  similar  to  the  Cartesian 
formulation  which  will  be  discussed  in  the  next  subsection. 
How  does  it  differ  from  the  Cartesian  formulation  can  be 
found  in  appendix  D.  More  details  on  this  formulation  can 
be  found  in  [Ref.  8]. 
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** '       Cartesian  Coordinates  Formulation 

This  is  the  traditional  way  of  formulating  the 
problem.   The  state  vector  consists  of: 

1.  Range  in  x-direction 

2.  Velocity  component  in  x-direction 

3.  Range  in  y-direction 

4.  Velocity  component  in  y-direction. 

The  state  equation  is  linear  and  the  measurement  equation  is 
now  the  nonlinear  part.  However  the  exhibited  nonlinearity 
is  easily  circumvented  without  complicated  or  lengthy  compu- 
tations as  it  will  be  shown  in  the  next  section. 

Finally  the  cartesian  coordinate  formulation  will  be 
adapted  in  the  following  discusion  mainly  because  of  its 
simplicity . 

C.   DESCRIPTION  OF  THE  FILTER  IN  CARTESIAN  COORDINATES 

1 .   Derivation  of  the  State  Equations 

If  we  will  consider  the  geometric  configuration  of 
Figure  1  and  with  the  restriction  of  target  and  tracker 
being  in  the  same  horizontal  plane,  the  Cartesian  formula- 
tion state  vector  may  contain  relative  ranges  and  relative 
velocities  in  X  and  Y  directions.  The  state  vector  that  will 
be  followed  in  this  analysis  is: 


x,  (t) 
xa  (t) 
x3(t) 
x<,(t) 


x(t) 
v,  (t) 

y(t) 

Vy(t) 


(4.1) 


with: 
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x(t)  xt(t)-x0(t) 

v*  (t)  vxt  (t)-vxo  (t) 

y(t)  yt(t)-y0(t) 

Vy  (t)  Vyt  (t)-Vyo  (t) 


(4.2) 


where  xt ( t ) ,yt  ( t )  ,  vxt  (t),vvt  (t)  are  the  target  absolute 
components  of  position  and  velocity  in  X  and  Y  directions, 
and  x0(t),ye  (t),vKO  (t),  and  vyo  (t)  are  the  tracker  absolute 
components  of  position  and  velocity.  The  linear  differen- 
tial equations  of  motion  of  the  model  are  given  by: 


• 
x,  (t) 
xa(t) 

x5(t) 
x4(t) 


xt(t) 

a,(t) 

x„(t) 

ay(t) 

(4.3) 


with 


a,  (t 


(t> 


a#t  (t)-a-0(t) 
ayt  (t)-ayo (t) 


(4.4) 


where  a.x  ( t )  and  ay  ( t )  are  the  relative  accelerations  in  both 
directions,  and  axt  (t),ay4  (t),aKO  (t)  and  ayo  (t)  are  the 
corresponding  absolute  accelerations  of  target  and  tracker 
in  both  directions  correspondingly. 

The  solutions  of  the  differential  equations  above  in 
matrix  notation  give: 


x(t)  =  A(t,tO)x(tO)  +  u(t   1) 


(4.5) 
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with 


(t.O" 


1 

(t-tO) 

0 

0 

0 

1 

0 

0 

0 

0 

1 

(t-tO) 

0 

0 

0 

1 

(4.6) 


and 


U 


(t/t.-) 


u,  (t,tO) 
u3  (t,tO) 
u3  (t,tO) 
uv  (t,tO) 


/(t->)ax  (>)d> 

to 

,<■ 

/(t->)ay  (>)d> 

to 

7ay  (>)d> 


(4.7) 


and  (tO)  denotes  any  arbitrary  fixed  value  of  time. 

Although  (4.5)  is  valid  for  unconstrained  vehicle 
motion,  solution  requirements  necessitate  that  the  bearings- 
only  target  motion  analysis  be  formulated  under  the  restric- 
tive assumption  of  constant  target  velocity.  [Ref.  9].  In 
this  case  ax<t(t)  and  ayt(t)  become  zero  and  u(t,tO)  reduces 
to  a  deterministic  input  vector  which  depends  only  upon  the 
tracker's  acceleration  (maneuvers).   So 


u(t,tO)  =  -u0 (t,tO) 


(4.8) 


where : 
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Uo(t/t^  = 


"o.  (t,tO) 

uoi  (t,tO) 
u0i  (t,tO) 
".»  (t,tO) 


f 


j](t->)aX0C»d> 
Ja*o  (^)d^ 
i(tt->)ayo  (^)d^ 


(4.9) 


2 .   Derivation  of  the  Measurement  Equation 

The   measurement  process   is  described   by  a   scalar 
time  varying  equation  of  the  form: 


•P(t)  =  h[x(t)]  +  n(t) 


(4.10) 


where 


h[x(t)]=  arctan  xs(t)/xi(t) 


(4.11) 


and  -P  ( t )  represents  the  measured  target  bearing  corrupted 
by  additive  measurement  noise  n(t).  It  is  assumed  that  n(t) 
is  a  white  noise  with  zero  mean  and  known  variance 
i.e., 


d2, 


E[n(t)]=  0 


(4.12) 


and 


E[n(t)n(t+»]  =  - 


<5        A=  ° 


S  *o 


(4.13) 


D 


THE  DISCRETE  TIME  MODEL 


•  The   previously   defined   model    in   discrete   form   is 
described  by: 


38 


x(k+l)  =  A(k)x(k)  -  u(k) 


(4.14) 


and 


f (k)  =  h[x(k)]  +  n(k) 


(4.15) 


where : 

x(k)  is  the  (4x1)  state  vector  consisted  from  rela- 
tive range  and  velocity  of  the  target  in  X  and  Y  directions. 

A(k)  is  the  (4x4)   state  transition  matrix  which  is 
constant  and  given  by: 


A(»o    = 


1 

T 

0 

0 

0 

1 

0 

0 

0 

0 

1 

T 

0 

0 

0 

1 

(4.16) 


u(k)   is  the  (4x1)    vector  of  deterministic  inputs 
due  to  tracker's  movement  and  given  by  Equation  (4.9). 


$00 

at  time  t (k) . 


is  the  scalar  noisy  bearing  measurement  taken 


n(k)   is   the  scalar  additive  measurement   noise  at 
time  t(k). 

Equation  (4.14)  assumes  that  the  target  moves  with  zero 
acceleration,  (non  maneuvering).  Also  it  is  assumed  that  the 
additive  measurement  noise  n(k)  has  zero  mean  and  a  known 
variance  d2(k).  Finally  an  initial  estimate  of  the  state 
vector  and  its  error  covariance  matrix  is  presumed  to  be 
given. 
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The  extended  Kalman   filter  technique  is  applied   to  the 
problem  and  yields: 

x(0/0)   is  the  initial  estimate  of  the  state  vector 
which  is  considered  to  be  given. 

P(0/0)   is   the  initial   estimate  error   covariance 
matrix  which  is  also  considered  to  be  given. 


x(k/k-l)  =  A(k)x(k-l/k-l)-w(k) 


(4.17) 


is  the  projection  ahead  of  the  estimated  state  vector. 


P(k/k-l)  =  A(k)P(k-l/k-l)A' (k)  +  Q(k) 


(4.18) 


is  the  projection  of  the  error  covariance  matrix  and  Q(k)  is 
the  maneuver  excitation  covariance  matrix  (assumed  zero  if 
the  target  does  not  maneuver) . 


H(k)  = 


3, 


x=x(k/k-l) 
is  a  (1x4)  matrix  given  by 


(4.19) 


H(k)  =  [x3/(x12+x^),0,-xI/(xt2+x.2),0] 


x=x(k/k-l) 


G(k)=P(k/k-l)Hf (k)[H(k)P(k/k-l)H' (k)+o2(k)] 


-i 


(4.20) 


(4.21) 


is  the  gain  equation 


•x(k/k)  =    x(k/k-l)+G(k)[^(k)-hx(k/k-l)] 


(4.22) 


is  the  update  equation  and 
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P(k/k)  =  [I-G(k)H(k)]P(k/k-l)  (4.23) 

is  the  error  covariance  update  equation. 

The  above  algorithm  was  formulated  in  computer  simula- 
tion program  as  in  Appendix  B  and  tested  for  the  situations 
shown  in  Figure  4.1  and  Figure  4.2. 

In  the  first  case  (Figure  4.1), the  target  was  moving 
from  east  to  west  on  a  constant  course  and  speed  of  20 
m/sec,  while  the  tracker  was  maneuvering  following  a  sinuso- 
dial  track  with  main  course  from  east  to  west  also,  and  a 
velocity  of  10  m/sec  in  x-direction.  The  target  had  a  rela- 
tive velocity  of  10  m/sec  with  respect  the  tracker  in  the 
X-axis  and  0  m/sec  in  the  Y-axis. 

In  the  second  case  (Figure  4.2),  the  target  was  moving 
as  the  first  case  but  the  tracker  was  following  a  circular 
path  of  radius  2000  m  with  a  turning  rate  of  2° /sec. 

The  measurement  error  was  taken  as  zero  mean  and  0.1 
covariance  and  the  measurement  interval  1  sec.  The  behavior 
of  the  filter  is  displayed  in  the  following  Figures  and  is 
considered  to  be  satisfactory. 
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V.  MANEUVERING  TARGET 

Up  to  this  time  we  made  the  assumption  that  the  target 
does  not  maneuver.  However  in  real  world  applications  this 
is  not  the  usual  case  and  hence  the  assumption  is  unreason- 
able. Specifically,  in  the  main  application  area  of  the 
bearings-only  tracking,  i.e.,  in  the  A.S.W  scene,  it  is 
expected  that  the  target  will  not  keep  constant  velocity  but 
instead  it  will  command  some  kind  of  zig-sag  during  the 
normal  open  sea  transit  and  strong  maneuvering  or  evasion 
after  detection  of  a  potential  threat.  It  is  evident  thus 
that  there  is  a  need  to  accommodate  the  maneuvering  case. 

A.   POSSIBLE  APPROACHES 

There  are  various  approaches  relative   to  the  problem  in 
general.   Some  found  in  the  literature  are  following: 

1 .   Variable  Dimension  Filter 

In  this  case,  the  filter  operates  in  its  normal  mode 
in  the  absence  of  any  maneuvers.  A  detection  scheme  is  used 
to  determine  that  a  maneuver  is  indeed  occuring.  Once  a 
maneuver  is  detected,  a  different  state  model  is  used.  The 
extent  of  the  maneuver  as  detected  is  then  used  to  yield  an 
estimate  for  the  extra  state  components.  The  tracking  is 
then  continued  with  the  augmented  state  model  until  it  will 
be  reverted  to  the  normal  model  by  another  decision.  The  two 
models  are  a  constant  velocity  and  a  constant  acceleration 
model  for  the  maneuvering  case.  Details  on  the  analysis  of 
that  method  can  be  found  in  [Ref.  10]. 
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2.  Expanded  Number  of  States 

In  this  case  the  model  includes  the  acceleration 
component  in  it.  This  method  has  the  disadvantage  that  if 
the  target  does  not  have  acceleration,  using  a  third  order 
model  increases  the  estimation  errors  for  both  position  and 
velocity  [Ref.  10].  Also, the  computational  load  increases 
drastically  by  augmenting  the  model  by  one  term. 

3 .  Modeling  Target   Acceleration  as   Random  Process   of 
Known  Form 

This  method  is  based  on  the  fact  that  the  target 
acceleration  and  thus  the  target  maneuver,  is  correlated  in 
time;  i.e.,  if  the  target  is  accelerating  at  time  t,  it  is 
likely  to  be  accelerating  at  time  t+&tau  for  sufficiently 
small  £.  A  typical  representative  model  of  the  correla- 
tion function  r(  )  associated  with  the  target  acceleration 
is  given  by: 


r(t)  =  E  [a(t)a(t+£)]  =  cr2e     ,a>0  (5.1) 

where  (o*2)  is  the  variance  of  the  target  acceleration  and 
(a)  is  the  reciprocal  of  the  maneuver  time  constant.  The 
maneuver  excitation  covariance  matrix  Q(k)  then  depends  on 
the  correlation  function  r('fc), which  also  depends  on  the  type 
of  the  target . 

The  above  formulation  includes  the  acceleration  term 
in  the  state  vector.  So  the  performance  of  the  filter  is 
degraded  by  the  computational  overhead.  The  quality  of  the 
estimate  is  also  degraded  when  the  target  is  moving  with 
constant  velocity. 

Analysis  of  the  above  method  can  be  found  at 
[Ref.  11]. 
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4.   Use   Variable  Maneuver   Excitation  Error   Covariance 
Matrix 

The  filter  is  modeled  as  a  second  order  and  it  does 
not  include  acceleration  term  in  it.  The  idea  is  [Ref.  12]. 
to  use  a  set  of  different  values  for  the  forcing  input 
covariance  Q(k) .  The  filter  monitors  the  innovation  error 
in  the  equation: 

x(k/k)  =  x(k/k-l)  +  G(k)[?(k)  -  h(x(k/k-l))]       (5.2) 

i.e.,  the  term  [$(k) -h(x(k/k- 1 ) ) ]  in  every  iteration.  If 
that  error  becomes  larger  than  a  predetermined  threshold, 
that  means  that  the  received  bearing  measurement  does  not 
agree  with  that  the  filter  generated  and  was  supposed  to 
receive.  Correspondingly,  the  estimated  vector  does  not 
agree  with  the  actual.  So  the  filter  assumes  that  the  target 
made  a  maneuver.  Depending  on  the  size  of  the  innovation 
error,  a  value  for  the  excitation  covariance  matrix  Q(k)  is 
applied  to  the  error  covariance  propagation  equation: 

P(k/k-l)  =  A(k)P(k/k-l)A' (k)  +  Q(k)  (5.3) 

The  effect  of  the  above  is  to  increase  the  uncertainty  of 
the  filter  which  consequently  causes  an  increase  of  the  gain 
G(k).  The  bigger  the  G(k)  the  more  the  filter  "believes" 
the  measurements  rather  than  the  previous  estimates. 

So  the  filter  is  "partially"  reinitialized.  By 
partially  is  meant  that  the  new  initial  estimates  of  the 
state  vector  and  specifically  the  range  terms  are  very  close 
to  the  real  ones  estimated  just  before  the  maneuver.  Thus 
the  filter  has  good  conditions  to  start  over  and  estimate 
the  new  state  after  the  maneuver. 
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B 


BEARINGS -ONLY  TRACKING  WITH  MANEUVERING  TARGET. 


From  the  previously  mentioned  methods  of  dealing  with 
maneuvering  targets,  we  are  going  to  develop  the  last  one 
i.e.,  that  of  using  a  variable  maneuver  excitation  error 
covariance  matrix  Q(k) . 

This  method  uses  a  four-state  model,  so  it  is  faster 
than  the  others  using  a  six-state  models  and  is  the  simplest 
of  all.  Actually  only  a  few  extra  lines  of  program  are 
added  to  that  of  a  nonmaneuvering  target. 

1.   Determination  of  the  Q(k)  Matrix 

If  we  will  suppose  that  the  target  made  a  maneuver, 
(acceleration  (a))  during  the  state  propagation  time  from 
(k)  to  (k+1),  in  one  direction  say  X,  then  the  error  intro- 
duced to  our  propagated  estimate  in  the  range  term  will  be 
(l/2)aT2  and  the  error  introduced  in  the  velocity  term  will 
be  aT .  Combining  that  fact  in  both  direction  and  with  the 
assumption  that  an  acceleration  in  X  is  uncorrelated  to  an 
acceleration  in  Y  the  resulting  Q(k)  is  given  by: 


Q(k)  =     f(k)E[w(k)w'(k)]f  '(k) 


(5.4) 


where 


%   _ 


7_ 


f(k)  = 


(5.5) 


o 
o 
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2  .   Simulation  Results  for  Cases  3  and  4  with  w=  1 ,  <5Z=0  .  1 

The  above  method  was  modeled  and  simulated  in  the 
computer.  Two  geometric  configurations  of  target  and 
tracker  as  shown  in  Figures  (5.1)  and  (5.2)  were  tested. 
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In  the  case  3  the  target  was  following  a  steady  course  from 
east  to  west  and  a  constant  speed  of  20  m/sec.  At  the  700th 
second  it  changed  course  to  the  right  and  speed  components 
to  12  m/sec  in  X  and  -10  m/sec  in  Y  direction.  After 
another  700  seconds  it  changed  course  again  that  time  from 
west  to  east  and  resumed  a  speed  of  20  m/sec.  The  tracker 
was  moving  as  in  case  1  of  the  nonmaneuvering  target.  The 
intervals  displayed  in  the  Figures  (5.1)  and  (5.2)  corre- 
spond to  time  intervals  of  100  seconds. 

In  the  case  4  the  target  was  following  the  same 
track  as  in  case  3  but  that  time  the  tracker  was  maneuvering 
as  in  case  2. 

In  the  following  simulations  the  measurement  error 
was  supposed  to  have  zero  mean  and  0.1  variance.  The  meas- 
urement interval  was  again  taken  as  1  sec  which  is  also 
considered  as  reasonable  for  a  real  application. The  (W)  was 
taken  equal  to  1.0. 

The  simulation  program  (Appendix  C)  for  the  above 
conditions  gave  the  results  shown  in  the  following  Figures 
5.3  to  5.16.  In  both  cases  the  filter  detected  the  maneuver 
and  very  rapidly  after  approximately  300  seconds  estimated 
the  new  target  parameters.  The  fluctuations  of  the  errors 
due  to  the  target  maneuver  are  smaller  than  those  during  the 
first  initialization  of  the  problem.  This  can  be  explained 
by  the  fact  that  after  the  target  maneuver  detection  the 
filter  had  an  "accurate"  reinitialization  state  from  the 
previous  tracking. 
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3    Filter  Behavior  Under  Different  Values  of  w  and  d2 

In  order  to  investigate  the  behavior  of  the  filter 
for  more  extreme  conditions,  simulations  where  conducted 
with  various  values  of  measurement  error  variance  (cf2)  and 
various  values  of  (w) .  The  following  combinations  where 
tested  for  the  case  3  configuration  and  the  range  error  was 
obtained  in  each  of  the  combinations. 


w  d_^ 

0.1  0.5 

0.1  2.0 

0.1  4.0 

1.0  0.5 

1.0  2.0 

1.0  4.0 

3.0  0.5 

3.0  2.0 

3.0  4.0 

10.0  0.5 

10.0  2.0 

10.0  4.0 


In  the  following  Figures  5.17  to  5.28  the  filter 
behavior  is  displayed.  It  is  characteristic  that  the  filter 
tracking  accuracy  and  quality  is  related  to  both  values  of 
(w)  and  (d2).  For  the  specific  configuration  it  came  out 
that  if  the  (d2)  was  more  than  0.5  then  the  filter  was  very 
sensitive  to  the  value  of  (w) .  The  best  results  were 
obtained  with  the  smallest  tested  value  of  w=0.1.  This 
should  be  expected  because  in  the  case  that  the  measurement 
noise  is  too   big  and  we  additionally   introduce  uncertainty 
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due  to  the  target  maneuver,  then  the  filter  assumes  a  lot  of 
uncertainty  and  at  any  time  behaves  erratically  following 
the  noisy  received  measurements.  It  seems  that  for  each 
kind  of  target  and  environmental  condition  (i.e.  measurement 
noise  variance)  there  will  be  an  optimal  (w)  to  account  for 
the  target  maneuvers. 
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VI.  CONCLUSIONS 

The  proposed  way  of  solving  the  problem  of  tracking  a 
maneuvering  target  using  noisy  bearings-only  measurements 
was  tested  and  it  exhibited  satisfactory  behavior.  The  main 
characteristics  of  it  are: 

1.  The  filter  responds  satisfactorily  in  the  case 
that  the  target  maneuvers.  The  filter  appears  to  be  very 
sensitive  to  the  value  of  o2.  The  results  were  satisfactory 
up  to  the  value  of  o2=0.5.  After  that  the  behavior  of  the 
filter  depends  very  much  on  the  value  of  w.  The  smaller  the 
value  of  w  the  better  the  filter  tracks. 

2.  The  design  is  simple  and  almost  no  extra  compu- 
tational power  is  needed  beyond  that  of  a  nonmaneuvering 
target  filter. 

3.  The  estimation  is  accurate  for  a  nonmaneuvering 
target  as  well,  and  it  does  not  pay  the  overhead  of  reduced 
accuracy  as  the  other  methods  do  in  the  nonmaneuvering  case. 

4.  Some  other  target- tracker  configurations  were 
tested  which  are  not  referenced  in  the  previous  chapters.  In 
some  of  them  the  filter  exhibited  disability  to  track  the 
target.  In  those  cases  the  characteristic  event  was  that  the 
target  was  moving  in  such  a  way  that  even  the  tracker's 
maneuvers  did  not  cause  significant  changes  in  the  measured 
bearings.  So  the  tracker  maneuvers  are  very  important  in  the 
bearings-only  tracking  problem.  They  must  be  such  that  will 
cause  changing  bearing  rates.  Of  course  the  tracker's 
maneuvers  are  restricted  by  various  factors  as  speed  capa- 
bility, tactical  situation,  intentions  (evade  or  attack), 
etc . 
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Possible  subjects  for  further  investigation: 

1.  Analytically  how  does  the  filter  behave  in  a 
variety  of  tracker- target ,  w  -  d2  configurations?  For 
example,  for  a  given  value  of  (tf2),  what  is  the  optimal  (w)? 

2.  In  the  simulations  the  tracker  was  supposed  to 
move  with  a  continuously  changing  course  which  is  not  the 
real  case.  Also  the  tracker  was  supposed  to  assume  huge 
amounts  of  acceleration  during  its  maneuvers,  i.e.  it  was 
supposed  to  change  course  and  speed  in  one  second  which  also 
is  not  realistic.  How  does  this  assumption  differ  from  the 
real  case? 

3.  Investigate  the  tracker  motion  under  realistic 
constraints  with  the  requirement  of  obtaining  tactical 
advantage  and  simultaneously  providing  needed  bearing  rate 
to  accurately  solve  the  tracking  problem. 

4.  Investigate  the  effect  of  assuming  realistic 
constraints  on  target  motion. 

In  this  Thesis  we  dealt  with  the  problem  of  maneuvering 
target  passive  tracking  using  a  simple  method.  The  first 
results  are  satisfactory,  however  the  method  needs  further 
detailed  investigation  for  even  better  performance. 
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APPENDIX  A 
SIMPLE  EXAMPLE  SIMULATION  PROGRAM 

REAL*4  P(4,4),H(2,4),HT(4,2),F(4,4),FT(4,4)J 
*S7(4,4), 
*X(4,1),T,TT,Z(2,1),XPR(4,1),PPR(4,4),Q(4,4),DET, 

*G(4,2), 

*hX(2,l),ZHX(2,l),Sl(4,2),S2(2,2),S6(2,2),GH(4,4), 
*FX(4,1) ,R1,R2,XI(4,1),R(2,2) ,W,FPUP(4,4) ,FPUPFT(4,4) , 
*A1,A2,A3,R3 
INTEGER  N,M,KK,I,J,K,L,NR,S 

N  =  4 

W=0.5 

M=l 

S  =  2 

NR=15 

DO  1  1=1, N 

DO  1  J=1,N 

F(I,J)=0. 

F(l,l)=l. 
F(l,2)=l. 

F(2,2)=l. 

F(3,3)=l. 

F(3,4)=l. 

F(4,4)=l. 

DO  2  1=1, N 

DO  2  J=1,N 

FT(I,J)=F(J,I) 

DO  3  I=1,N 

DO  3  J=1,N 

S7(I,J)=0. 

DO  4  1=1, N 
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DO  4  J=1,N 

4  S7(I,I)=1. 
X(l,l)=10000. 
X(2,l)=0. 
X(3,l)=0. 
X(4,l)=1600. 

R(l,l)=l. 

R(l,2)=0. 

R(2,l)=0. 
R(2,2)=l. 
DO  5  1=1, N 
DO  5  J=1,N 

5  P(I,J)=0. 
P(l,l)=1000. 
P(2,2)=1000. 
P(3,3)=1000. 
p(4,4)=1000. 
DO  6  1=1, S 
DO  6  J=1,N 

6  H(I,J)=0. 

H(l,l)=l- 

H(2,3)=l. 
DO  7  1=1, S 
DO  7  J=1,N 

7  HT(J,I)=H(I,J) 
DO  71  1=1, N 

DO  71  J=1,N 
71     Q(I,J)=0. 

Q(1,1)=.25*W 

Q(3,3)=.25*W 

Q(1,2)=.5*W 

Q(3,4)=.5*W 

Q(2,1)=.5*W 

Q(4,3)=.5*W 

Q(2,2)=W 
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Q(4,4)=W 

DO  999  KK=1,NR 

T = FLOAT (KK) 

L=KK-1 

TT=FLOAT(L) 

Z ( 1 , 1 ) = 10000 . -COS ( . 157*TT ) 

Z (2,1)= 10000. *SIN( . 157*TT) 

CALL  MM(P,HT,S1,N,N,S) 

CALL  MM(H,S1,S2,S,N,S) 

DO  8  1=1, S 

DO  8  J=1,S 

8  S2(I,J)=S2(I,J)+R(I,J) 
DET=S2(1,1)*S2(2,2)-S2(1,2)*S2(2,1) 
S6(1,1)=S2(2,2)/DET 
S6(1,2)=-S2(1,2)/DET 
S6(2,1)=-S2(2,1)/DET 
S6(2,2)=S2(1,1)/DET 

CALL  MM(S1,S6 ,G,N,S,S) 
CALL  MM(H,X,HX,S,N,M) 
DO  9  1=1, S 
DO  9  J=1,M 

9  ZHX(I, J)=Z(I, J)-HX(I, J) 
CALL  MM(G,ZHX,XI,N,S,M) 
DO  10  1=1, N 

DO  10  J=1,M 

10  X(I,J)=X(I,J)+XI(I,J) 
CALL  MM(G,H,GH,N,S,N) 
DO  11  1=1, N 

DO  11  J=1,N 

11  IGH(I,J)=S7(I,J)-GH(I,J) 
CALL  MM(IGH,P,PUP,N,N,N) 
CALL  MM(F,X,XPR,N,N,M) 
CALL  MM(F,PUP,FPUP,N,N,N) 
CALL  MM(FPUP,FT,FPUPFT  N,N,N) 
DO  13  1=1, N 
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DO  13  J=1,N 

13  PPR(I,J)=FPUPFT(I,J)+Q(I,J) 

WRITE (6,106)Z(1,1),Z(2,1),X(1,1),X(3,1),  XPR (1,1), 
*XPR(3,1) 
X(1,1)=XPR(1,1) 

X(2,1)=XPR(2,1) 
X(3,1)=XPR(3,1) 

X(4,1)=XPR(4,1) 
DO  14  1=1, N 
DO  14  J=1,N 

14  P(I,J)=PPR(I,J) 
106  FORMAT(6(F8.1)) 
999    CONTINUE 

STOP 
END 

SUBROUTINE  MM(A,B , C ,N1 , N2 ,N3 ) 
REAL-4  A(N1,N2) , B (N2 ,N3 ) , C (Nl ,N3 ) 
DO  100  1=1, Nl 
DO  100  K=1,N3 
C(I,K)=0. 
DO  100  J=1,N2 
100    C(I,K)=C(I,K)+A(I,J)*B(J,K) 
RETURN 
END 
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APPENDIX  B 
B.O.T  NONMANEUVERING  TARGET  SIMULATION  PROGRAM, 

REAL*8  P(4,4),H(1,4),HT(4,1),Q(4,4),AX, 

*XA(1,1),YA(1,1), 

*RI(1,1) ,VV(4000,4) ,RR(4000) ,S7(4,4) , 
*GRT(4,4) ,VXA(1,1) ,VYA(1,1) 
*,V( 1,4000), Z( 1, 1),Y(1,1),S1(4,1), 
*S2(1,1) ,XI(4, 1) ,TT,X1,X3,E1,UU, 
*HH(1,1),HI(4,1),X(4,1),G(4,1), 
*S6(1,1),GY(4,1),HX(1,1),TY,W, 
*GHX(4,1),F(4,4),FT(4,4),XPR(4,1), 
*PPR(4,4),FP(4,4),GT(1,4),R(1,1), 
*GH(4,4) ,IGH(4,4) ,IGHT(4,4) , PUP (4, 4) , 
- IGHP (4,4), IGHPT ( 4 , 4 ) , GR ( 4 , 1 ) 
INTEGER  N,M,NN,NR,L,KK,I,J,K,NS 

N=4 

M=l 

NN=1 

W=3.D0 

NR=2000 

NS=4000 

SB=. 1D0/57.295779D0 

SX=50.  DO 

SY=50.  DO 

SVX=1.  DO 

SVY=1.  DO 

DS=211133.D0 

DS1=333333.D0 

HHH=O.DO 

DO  1  J=1,N 
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DO  1  1=1, N 
Q(I,J)=0.D0 

1  F(I,J)=0.D0 
F(1,1)=1.D0 
F(1,2)=1.D0 
F(2,2)=1.D0 
F(3,3)=l.DO 
F(3,4)=1.D0 
F(4,4)=1.D0 
DO  2  I=1,N 
DO  2  J=1,N 

2  FT(I,J)=F(J,I) 
C 

C      GENERATION  AND  STORAGE  OFF  I. C. NOISE 
DO  10  1=1, N 
CALL  GGNML  (DS,NS,RR) 
DO  10  J=1,NN 

10  VV(J,I)=RR(J) 
C 

C      MAKE  MATRIX  S7= IDENTITY. 
DO  11  1=1, N 
DO  11  J=1,N 

11  S7(I,J)=0.D0 
DO  12  1=1, N 

12  S7(I,I)=1.D0 
C 

C   START  THE  FIRST  RUN, INITIAL  STATE  VALUE 

DO  99  JJ=1,NN 

X(l,l)=-4000.  DO 

X(2,1)=+12.D0 

X(3,1)=5000.D0 

X(4,1)=2.D0 
C 
C  DEFINE  R  AND  RI  MATRICES. 

R(1,1)=SB**2 
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RI(1,1)=1.D0/R(1,1) 

C  INITIALIZE  P  MATRIX 
DO  13  1=1, N 
DO  13  J=1,N 

13  P(I,J)=0.D0 

•   P(1,1)=SX**2 
P(2,2)=SVX**2 
P(3,3)=SY**2 
P(4,4)=SVY**2 
C      GENERATION  OFF  MEASUREMENT  NOISE  -  STORAGE. 
DO  14  1=1, M 
CALL  GGNML(DS1,NS,RR) 
DO  14  J=1,NR 

14  V(I,J)=RR(J) 
C 

C   TIME  EVOLUTION 
C 

DO  9  99  KK=1,NR 

T=DFLOAT(KK) 

L=KK-1 

TT=DFLOAT(L) 
C   GENERATION  OFF  MEASUREMENT  DATA 

Xl= -  5000 . D0+ 10 . DO *TT 

X3 = 8000. D0  + 2000. DO*DCOS( 0.0 35  D0*TT) 

UU=X1/X3 

Z ( 1 , 1 ) =DATAN2 (XI , X3 ) 
C      ADD  NOISE 

Y(1,1)=Z(1,1)+SB*V(1,KK) 
C  PROJECTION  OF  X:  XPR  =  X(K+1/K)  =  F  *  X(K/K)+  D*U 

CALL  MM(F,X,XPR,N,N,M) 
C 

AX=0.D0 

VY  = -  70 . DO-DSIN ( 0 . 035D0* (TT  +  0 . DO ) ) 

AY=-2.45  D0*DCOS(0.O35D0*TT) 
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D(1,1)=0.D0 
D(2,1)=0.D0 

D(3,l)=AY/2.0  DO 
D(4,1)=AY 

C 

DO  77  1=1, N 
DO  77  J=1,M 

77  XPR(I,J)=XPR(I,J)+D(I,J) 
C 

C  PROJECTION  OF  P  :  PPR  =  P(K+1/K)  =  F  *  P(K/K)  *  FT  +  Q 
CALL  MM(F,P,FP,N,N,N) 
CALL  MM(FP,FT,PPR,N,N,N) 
DO  78  1=1, N 
DO  78  J=1,N 

78  PPR(I,J)=PPR(I,J)+Q(I,J) 
X(1,1)=XPR(1,1) 
X(2,1)=XPR(2,1) 
X(3,1)=XPR(3,1) 
X(4,1)=XPR(4,1) 

C 

DO  6  8  1=1, N 

DO  68  J=1,N 
68     P(I,J)=PPR(I,J) 
C    H-  MATRIX 

U=X(1,1)**2+X(3,1)**2 

H(1,1)=X(3,1)/U 

H(1,2)=0.D0 

H(l,3)=-X(l,l)/U 

H(1,4)=0.D0 
C  H-  TRANSPOSE  MATRIX 

HT(1,1)=H(1,1) 

HT(2,1)=H(1,2) 

HT(3,1)=H(1,3) 

HT(4,1)=H(1,4) 
C  MEASUREMENT  UPDATING 
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C    COMPUTATION  OF  GAIN  MATRIX  G=P*HT/ (H*P*HT+R) 

CALL  MM(P,HT,S1,N,N,M) 

CALL  MM(H,S1,S2,M,N,M) 

DO  23  1=1, M 

DO  23  J=1,M 
23     S2(I,J)=S2(I,J)+R(I,J) 

S6(1,1)=1.D0/S2(1,1) 

CALL  MM(S1,S6,G,N,M,M) 
C 

GT(1,1)=G(1,1) 

GT(1,2)=G(2,1) 

GT(1,3)=G(3,1) 

GT(1,4)=G(4,1) 
C  ERROR  COVARIANCE  MATRIX  UPDATE: 
C  P(K+1/K+1)  =  {I-G"H}"P(K+1/K) 

CALL  MM(G,H,GH,N,M,N) 

DO  73  1=1, N 

DO  73  J=1,N 
73     IGH(I,J)=S7(I,J)-GH(I,J) 

CALL  MM(IGH,P,IGHP,N,N,N) 

DO  75  1=1, N 

DO  75  J=1,N 
7  5     PUP(I,J)=IGHP(I,J) 

DO  76  1=1, N 

DO  76  J=1,N 
76     P(I,J)=PUP(I,J) 
C  STATE  UPDATE  AT  MEASUREMENT 
C      X(+)=X(-)+6*(Y-H(X(-)))  !!  BUT  FOR  E.K.F. 

CALL  MM(H,X,HX,M,N,M) 

HH(1,1)=Y(1,1)-DATAN2(X(1,1),X(3,1)) 

CALL  MM(G,HH,XI,N,M,M) 

DO  80  1=1, N 

DO  80  J=1,M 
80     X(I,J)=X(I,J)+XI(I,J) 

E1=DSQRT((X1-X(1,1))**2+(X3-X(3,1))-— 2) 
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E2=(X1-X(1,1)) 
E3=(X3-X(3,1)) 

TY=X(4,1)-VY-AY 

WRITE  (6,107)T,E2,E3,E1,X(2,1),TY 
107    FORMAT(6(3X(F14.4))) 
999    CONTINUE 

99  CONTINUE 
STOP 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
SUBROUTINE  MM(A,B , C ,N1 ,N2 ,N3 ) 
REAL-8  A(N1,N2) ,B(N2,N3) ,C(N1,N3) 
DO  100  1=1, Nl 
DO  100  K=1,N3 
C(I,K)=O.DO 
DO  100  J=1,N2 

100  C(I,K)=C(I,K)+A(I, J)-B(J,K) 
RETURN 
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APPENDIX  C 
B.O.T  MANEUVERING  TARGET  SIMULATION  PROGRAM. 

C 

REAL*8  P(4,4) ,H(1,4) ,HT(4,1) ,Q(4,4) ,AX,AY, 
*D(4,1),XA(1,1),YA(1,1),RI(1,1),VV(4000,4), 
*RR(4000),S7(4,4) ,GRT(4,4) ,VXA(1,1) ,VYA(1,1) , 
*V(lf4000),Z(lll),T(l>l),Sl(4,l),S2(l,l), 
»XI (4,1) ,TT,X1,X3,E1,UU,HH(1,1) , HI (4,1) ,X(4,1) , 
*G(4,1) ,S6(1,1) ,GY(4,1) ,HX(1,1) ,TY,HHH,W, 
*GHX(4,1),F(4,4),FT(4,4),XPR(4,1),PPR(4,4), 
*FP(4,4),GT(1,4),R(1,1),GH(4,4),IGH(4,4), 
*IGHT(4,4) , PUP (4, 4) ,IGHP(4,4) ,IGHPT(4,4) ,GR(4,1) 

INTEGER  N , M , NN , NR , L , KK , I , J , K , NS , HHHH , HHHHH 
C 

N=4 

M=l 

NN=1 

cccccccccccccccccc 

W=1.D0 
CCCCCCCCCCCCCCCCCC 

NR=2000 

NS=4000 
CCCCCCCCCCCCCCCCCC 

SB=.1D0/57.295779D0 
CCCCCCCCCCCCCCCCCC 

SX=50.  DO 

SY=50.  DO 

SVX=1.  DO 

SVY=1.  DO 

DS=211133.DO 

DS1=333333.D0 

HHH=O.DO 

106 


DO  1  J=1,N 
DO  1  1=1, N 
Q(I,J)=O.DO 

1  F(I,J)=0.D0 
F(1,1)=1.D0 
F(1,2)=1.D0 
F(2,2)=1.D0 
F(3,3)=1.D0 
F(3,4)=1.D0 
F(4,4)=1.D0 
DO  2  1=1, N 
DO  2  J=1,N 

2  FT(I,J)=F(J,I) 
C 

C      GENERATION  AND  STORAGE  OFF  I. C. NOISE 
DO  10  1=1, N 
CALL  GGNML  (DS,NS,RR) 
DO  10  J=1,NN 

10  VV(J,I)=RR(J) 

C      MAKE  MATRIX  S7= IDENTITY. 
DO  11  I=1,N 
DO  11  J=1,N 

11  S7(I,J)=0.D0 
DO  12  1=1, N 

12  S7(I,I)=1.D0 

C   START  THE  FIRST  RUN, INITIAL  STATE  VALUE 

DO  9  9  JJ=1,NN 

X(l,l)=-4000.  DO 

X(2,1)=+12.D0 

X(3,1)=5000.D0 

X(4,1)=2.D0 
C  DEFINE  R  AND  RI  MATRICES. 

R(1,1)=SB**2 

RI(1,1)=1.D0/R(1,1) 
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C  INITIALIZE  P  MATRIX 
DO  13  1=1, N 
DO  13  J=1,N 

13  P(I,J)=0.D0 
P(1,1)=SX**2 
P(2,2)=SVX**2 
P ( 3 , 3 ) = SY**2 
P(4,4)=SVY**2 

C      GENERATION  OFF  MEASURMENT  NOISE  -  STORAGE. 
DO  14  1=1,  M 
CALL  GGNML(DS1,NS,RR) 
DO  14  J=1,NR 

14  V(I,J)=RR(J) 
C   TIME  EVOLUTION 

DO  999  KK=1,NR 

T=DFLOAT(KK) 

L=KK-1 

TT=DFLOAT(L) 
C   GENERATION  OFF  MEASURMENT  DATA 

Xl= -  5000 . D0+ 10 . DO-TT 

X3  =  8000. D0  + 2000. D0*DCOS( 0.035  DO-TT ) +2000 . DO 

IF  (KK.LT. 700)  GO  TO  33 

Xl=-100.D0+3.D0*TT 

X3=8000 . DO  +  2000 . DO-DCOS (0 . 035  D0*TT) -  10 . DO-TT+9000 . DO 

IF  (KK.LT. 1400. AND. KK.GE. 700)  GO  TO  33 

Xl=46100.D0-30.D0*TT-6500.D0+6500.D0 

X3 = 8000. D0+ 2000. DO *DCOS( 0.035  DO *TT) +7000 .DO- 12000 .DO 
33     CONTINUE 

Z(1,1)=DATAN2(X1,X3) 
C      ADD  NOISE 

Y(1,1)=Z(1,1)+SB*V(1,KK) 
C  PROJECTION  OF  X:  XPR  =  X(K+1/K)  =  F  *  X(K/K)+  D-'U 

CALL  MM(F,X,XPR,N,N,M) 

AX=0.D0 

VY= -  70 . DO-DSIN (0 . 035D0- (TT  +  0 . DO ) ) 
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AY  = -  2 . 45  DO-DCOS (0 . 035D0*TT) 
D(1,1)=0.D0 

D(2,1)=0.D0 
D(3,l)=AY/2.0  DO 
D(4,1)=AY 

C 

DO  77  1=1, N 
DO  77  J=1,M 

77  XPR(I,J)=XPR(I,J)+D(I,J) 

C  PROJECTION  OF  P  :  PPR  =  P(K+1/K)  =  F  *  P(K/K)  *  FT  +  Q 
CALL  MM(F,P,FP,N,N,N) 
CALL  MM(FP,FT,PPR,N,N,N) 
IF  (KK.LT.600.)  GO  TO  86 

IF  (KK.GT.600.AND.HHHHH.LT. 1. )  GO  TO  6  6 
IF  (KK.GT.600.AND.HHHHH.GT. 1. )  GO  TO  6  7 

66  DO  79  1=1, N 
DO  79  J=1,N 

79     Q(I,J)=0.D0 
GO  TO  86 

67  Q(l,l)=.25  D0*W 
Q(l,2)=.5  D0*W 
Q(2,l)=.5  D0*W 
Q(2,2)=W 
Q(3,3)=.25  D0*W 
Q(3,4)=.5  DO-W 
Q(4,3)=.5  DO-W 
Q(4,4)=W 

86     CONTINUE 

DO  78  1=1, N 
DO  78  J=1,N 

78  PPR(I,J)=PPR(I,J)+Q(I,J) 
X(1,1)=XPR(1,1) 
X(2,1)=XPR(2,1) 
X(3,1)=XPR(3,1) 
X(4,1)=XPR(4,1) 
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DO  68  1=1, N 

DO  68  J=1,N 
68     P(I,J)=PPR(I,J) 
c   H-  MATRIX 

U=X(1,1)**2+X(3,1)**2 

H(1,1)=X(3,1)/U 

H(1,2)=0.D0 

H(l,3)=-X(l,l)/U 

H(1,4)=0.D0 
C  H-  TRANSPOSE  MATRIX 

HT(1,1)=H(1,1) 
HT(2,1)=H(1,2) 
HT(3,1)=H(1,3) 

HT(4,1)=H(1,4) 
C  UPDATING  AT  MEASUREMENT 
C    COMPUTATION  OF  GAIN  MATRIX  G=P*HT/ (H*P*HT+R) 

CALL  MM(P,HT,S1,N,N,M) 

CALL  MM(H,S1,S2,M,N,M) 

DO  23  1=1, M 

DO  23  J=1,M 
23     S2(I,J)=S2(I,J)+R(I,J) 

S6(1,1)=1.D0/S2(1,1) 

CALL  MM(S1,S6,G,N,M,M) 

GT(1,1)=G(1,1) 

GT(1,2)=G(2,1) 

GT(1,3)=G(3,1) 

GT(1,4)=G(4,1) 
C     ERROR    COVARIANCE     MATRIX    UPDATE : P (K+ 1/K+ 1) 
{I-G*H}*P(K+1/K)  {      } 

CALL  MM(G,H,GH,N,M,N) 

DO  73  1=1, N 

DO  73  J=1,N 
73     IGH(I, J)=S7(I, J)-GH(I, J) 

CALL  MM(IGH,P,IGHP,N,N,N) 
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DO  75  1=1, N 
DO  75  J=1,N 

75  PUP(I,J)=IGHP(I,J) 
DO  76  1=1, N 

DO  76  J=1,N 

76  P(I,J)=PUP(I,J) 

C  STATE  UPDATE  AT  MEASURMENT 

C      X(+)=X(-)+G-(Y-H(X(-) ))  !!  BUT  FOR  E.K.F. 

CALL  MM(H,X,HX,M,N,M) 

HH(1,1)=Y(1,1)-DATAN2(X(1,1),X(3,1)) 

HHH=HH(1, 1) -150. DO 

HHHH=SNGL(HHH) 

HHHHH  = I AB  S ( HHHH ) 

CALL  MM(G,HH,XI,N,M,M) 

DO  80  1=1, N 

DO  80  J=1,M 
80     X(I,J)=X(I,J)+XI(I,J) 

E1=DSQRT((X1-X(1,1))**2+(X3-X(3,1))**2) 

E2=(X1-X(1,1)) 

E3=(X3-X(3,1)) 

TY=X(4,1)-VY 

TX=X(2,1)+10.D0 
C      WRITE  (6,107)T,E2,E3,E1,X(2,1),X(4,1) 
C      WRITE  (6,107)T,X1,X3 

WRITE  (6,107)T,TX,TY,HH(1,1) 
107    FORMAT(6(3X(F14.4))) 
999    CONTINUE 
99     CONTINUE 

STOP 

END 

cccccccccccccccccccccccccccccccccccccccccccccc 

SUBROUTINE  MM(A,B , C ,N1 ,N2 ,N3 ) 
REAL- 8  A(N1,N2) ,B(N2,N3) ,C(N1,N3) 
DO  100  1=1, Nl 
DO  100  K=1,N3 
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C(I,K)=O.DO 
DO  100  J=l,  2 
100    C(I,K)=C(I,K)+A(I,J)*B(J,K) 

RETURN 

END 

END 
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APPENDIX  D 
PSEUDO-LINEAR  FORMULATION 


If  we  will  start  we  the  Cartesian  formulation  equations 


$(k)  =  h[x(k)]  ♦  n(k) 


(D.l) 


h[x(k)]  =  arctan[xx  (k)/x3  (k)] 


(D.2) 


E  [n(k)J  =  0 


E  [n(i)n(j)J  =  > 


a   Cw>   t  =  3 


Q  I*  J 

then  after  algebraical  manipulations  yield 


(D.3) 


(D.4) 


0  =  H(k)x(k)  +  R(k)n(k) 


(D.5) 


where 


H(k)  =  [cosj(k),  -sin^(k),  0,  0] 
R(k)s^/x12(k)  +  x  2(k) 


(D.6) 
(D.7) 


The  nonlinearity  has  been  embeded   in  the  measurement  noise 
If 


€ (k) =R(k)n(k) =ef f ective   measurement  noise   at  time   kT 

(D.8) 
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it  can  been  shown  [Ref.  13].     that  £(k)   has  the  following 
statistics : 

E  [6(k)J  =  0  (D.9) 

E  [e2(k)]  =  R2(k)cr2(k)  (D.10) 

Finally  the  pseudo- linear  model  is  analogous  to  that  of 
Cartesian  formulation  model  with  the  following  modifica- 
tions : 

1.  replacing  H(k)  with  that  given  by  equation  (  C.6) 

2.  replacing  cr  (k)  with  R(k/k- 1)  <J(k) 

3.  replacing  ^(k) -h  [x(k/k- 1)]  with  -H(k)x(k/k- 1)   f 
Detailed   analysis   on   the   subject   can   be   found   in 

[Ref.  8]. 
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